#include <common/miscutils.h>	// for limit
#include "pidregulator.h"

PidRegulator::PidRegulator()
	: config_()
	, integrator_(0.0)
	, satError_(0.0)
	, prev_feedback_(0.0)
	, output_(0.0)
	, scale_(1.0)
{}

PidRegulator::~PidRegulator() {}
	
void PidRegulator::setConfig(const PidRegulatorConfig &config) {
	if (config_ != config) {
		config_ = config;
		reset();
	}
}

void PidRegulator::setMinMax(float min, float max) {
	config_.min = min;
	config_.max = max;
}

void PidRegulator::setMinMax(float lim) {
	setMinMax(-lim, lim);
}

float PidRegulator::getMin() const {
	return config_.min;
}

float PidRegulator::getMax() const {
	return config_.max;
}

void PidRegulator::setScale(float scale) {
	scale_ = scale;
}

void PidRegulator::reset(float output) {
	integrator_ = (config_.ki != 0) ? limit(output, config_.min, config_.max) : 0;	// output tracking
	satError_ = 0;
	prev_feedback_ = 0;
}

float PidRegulator::operator()(float reference, float feedback) {
	float error = reference - feedback;
	float proportional = config_.kp * error * scale_;

	integrator_ += config_.ki * proportional + config_.kc * satError_;
	integrator_ = limit(integrator_, config_.min, config_.max);

	float derivative = config_.kd * (feedback - prev_feedback_) * scale_;
	prev_feedback_ = feedback;
	
	float outputPreSat = proportional + integrator_ + derivative;
	output_ = limit(outputPreSat, config_.min, config_.max);
	satError_ = output_ - outputPreSat;

	return getOutput();
}

float PidRegulator::operator()(float reference, float feedback, float d_feedback) {
	prev_feedback_ = feedback - d_feedback;
	return operator()(reference, feedback);
}

float PidRegulator::getOutput() const {
	return output_;
}
